#ifndef ZOROS_ROBOT_MANAGER_H
#define ZOROS_ROBOT_MANAGER_H

#include <mutex>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <std_msgs/Float64MultiArray.h>
#include <thread>
#include "damiao.h"
#include "elog/elog_interface.h"
#include "model/model_interface.hpp"
#include "plan/plan_interface.hpp"
#include <std_msgs/String.h>
#include "std_msgs/Int32.h"
#include "std_msgs/Float64.h"
#include <atomic>

#define CONTROL_FREQ 250

class RobotManager
{
public:
    RobotManager(ros::NodeHandle &nh);

    void start();

private:
    ros::NodeHandle nh_;
    ros::Publisher joint_pub_;
    ros::Publisher cmd_pub_;
    sensor_msgs::JointState joint_state_msg_;
    sensor_msgs::JointState cmd_msg_;
    std::thread joint_state_refresh_thread_;
    bool is_simulation_ = true;
    // bool is_simulation_;
    std::shared_ptr<ZORAL::ZoralModel> robot_model_;
    std::shared_ptr<ZORAL::ZoralPlan> robot_plan_;
    std::shared_ptr<damiao::Motor_Control> dm_;
    std::vector<damiao::Motor> motors_;
    pthread_mutex_t zoros_mutex_{};
    ros::Timer timer_;
    ros::Subscriber move_joint_subscriber_;
    ros::Subscriber move_pose_subscriber_;
    ros::Subscriber move_traj_subscriber_;
    ros::Subscriber move_line_subscriber_;
    ros::Subscriber move_nullspace_subscriber_;
    ros::Subscriber teach_move_subscriber_;
    ros::Subscriber trajectory_replay_subscriber_;
    ros::Subscriber joint_scan_subscriber_;
    bool running_;
    bool teach_mode_ = false;

    void initializeMotors();

    void publishJointStatesLoop();

    void commandMoveJointCallback(const std_msgs::Float64MultiArray::ConstPtr &msg);
    
    void commandMovePoseCallback(const std_msgs::Float64MultiArray::ConstPtr &msg);

    void commandMoveLineCallback(const std_msgs::Float64MultiArray::ConstPtr &msg);

    void commandMoveNullspaceCallback(const std_msgs::Float64::ConstPtr& msg);

    void commandMoveTrajCallback(const std_msgs::String::ConstPtr &msg);

    void commandTeachMoveCallback(const std_msgs::Int32::ConstPtr& msg);

    void commandTrajectoryReplayCallback(const std_msgs::String::ConstPtr &msg);

    void commandJointScanCallback(const std_msgs::Float64MultiArray::ConstPtr &msg);


};
#endif /*ZOROS_ROBOT_MANAGER_H*/
